#include "flight_control_sample.hpp"

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;

/*! Monitored Takeoff (Blocking API call). Return status as well as ack.
    This version of takeoff makes sure your aircraft actually took off
    and only returns when takeoff is complete.
    Use unless you want to do other stuff during takeoff - this will block
    the main thread.
!*/
bool monitoredTakeoff(Vehicle* vehicle, int timeout) {
  //@todo: remove this once the getErrorCode function signature changes
  char func[50];
  int pkgIndex;

  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    // Telemetry: Verify the subscription
    ACK::ErrorCode subscribeStatus;
    subscribeStatus=vehicle->subscribe->verify(timeout);

    if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
      ACK::getErrorCodeMessage(subscribeStatus, func);
      return false;
    }

    // Telemetry: Subscribe to flight status and mode at freq 10 Hz
    pkgIndex=0;
    int freq=10;

    TopicName topicList10Hz[]= {
      TOPIC_STATUS_FLIGHT,
      TOPIC_STATUS_DISPLAYMODE
    }

    ;
    int numTopic=sizeof(topicList10Hz) / sizeof(topicList10Hz[0]);
    bool enableTimestamp=false;

    bool pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList10Hz, enableTimestamp, freq);

    if ( !(pkgStatus)) {
      return pkgStatus;
    }

    subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, timeout);

    if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
      ACK::getErrorCodeMessage(subscribeStatus, func);
      // Cleanup before return
      vehicle->subscribe->removePackage(pkgIndex, timeout);
      return false;
    }
  }

  // Start takeoff
  ACK::ErrorCode takeoffStatus=vehicle->control->takeoff(timeout);

  if (ACK::getError(takeoffStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(takeoffStatus, func);
    return false;
  }

  // First check: Motors started
  int motorsNotStarted=0;
  int timeoutCycles=20;

  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    while (vehicle->subscribe->getValue<TOPIC_STATUS_FLIGHT>() !=VehicleStatus::FlightStatus::ON_GROUND && vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_ENGINE_START && motorsNotStarted < timeoutCycles) {
      motorsNotStarted++;
      usleep(100000);
    }

    if (motorsNotStarted==timeoutCycles) {
      std::cout << "Takeoff failed. Motors are not spinning."<< std::endl;

      // Cleanup
      if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
        vehicle->subscribe->removePackage(0, timeout);
      }

      return false;
    }

    else {
      std::cout << "Motors spinning...\n";
    }
  }

  else if (vehicle->isLegacyM600()) {
    while ((vehicle->broadcast->getStatus().flight < DJI::OSDK::VehicleStatus::FlightStatus::ON_GROUND) && motorsNotStarted < timeoutCycles) {
      motorsNotStarted++;
      usleep(100000);
    }

    if (motorsNotStarted < timeoutCycles) {
      std::cout << "Successful TakeOff!"<< std::endl;
    }
  }

  else // M100

    {
    while ((vehicle->broadcast->getStatus().flight < DJI::OSDK::VehicleStatus::M100FlightStatus::TAKEOFF) && motorsNotStarted < timeoutCycles) {
      motorsNotStarted++;
      usleep(100000);
    }

    if (motorsNotStarted < timeoutCycles) {
      std::cout << "Successful TakeOff!"<< std::endl;
    }
  }

  // Second check: In air
  int stillOnGround=0;
  timeoutCycles=110;

  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    while (vehicle->subscribe->getValue<TOPIC_STATUS_FLIGHT>() !=VehicleStatus::FlightStatus::IN_AIR && (vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF) && stillOnGround < timeoutCycles) {
      stillOnGround++;
      usleep(100000);
    }

    if (stillOnGround==timeoutCycles) {
      std::cout << "Takeoff failed. Aircraft is still on the ground, but the "
        "motors are spinning."
        << std::endl;

      // Cleanup
      if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
        vehicle->subscribe->removePackage(0, timeout);
      }

      return false;
    }

    else {
      std::cout << "Ascending...\n";
    }
  }

  else if (vehicle->isLegacyM600()) {
    while ((vehicle->broadcast->getStatus().flight < DJI::OSDK::VehicleStatus::FlightStatus::IN_AIR) && stillOnGround < timeoutCycles) {
      stillOnGround++;
      usleep(100000);
    }

    if (stillOnGround < timeoutCycles) {
      std::cout << "Aircraft in air!"<< std::endl;
    }
  }

  else // M100

    {
    while ((vehicle->broadcast->getStatus().flight !=DJI::OSDK::VehicleStatus::M100FlightStatus::IN_AIR_STANDBY) && stillOnGround < timeoutCycles) {
      stillOnGround++;
      usleep(100000);
    }

    if (stillOnGround < timeoutCycles) {
      std::cout << "Aircraft in air!"<< std::endl;
    }
  }

  // Final check: Finished takeoff
  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    while (vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>()==VehicleStatus::DisplayMode::MODE_ASSISTED_TAKEOFF || vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>()==VehicleStatus::DisplayMode::MODE_AUTO_TAKEOFF) {
      sleep(1);
    }

    if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
      if (vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_P_GPS || vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_ATTITUDE) {
        std::cout << "Successful takeoff!\n";
      }

      else {
        std::cout << "Takeoff finished, but the aircraft is in an unexpected mode. "
          "Please connect DJI GO.\n";
        vehicle->subscribe->removePackage(0, timeout);
        return false;
      }
    }
  }

  else {
    float32_t delta;
    Telemetry::GlobalPosition currentHeight;
    Telemetry::GlobalPosition deltaHeight=vehicle->broadcast->getGlobalPosition();

    do {
      sleep(4);
      currentHeight=vehicle->broadcast->getGlobalPosition();
      delta=fabs(currentHeight.altitude - deltaHeight.altitude);
      deltaHeight.altitude=currentHeight.altitude;
    }

    while (delta >=0.009);

    std::cout << "Aircraft hovering at "<< currentHeight.altitude << "m!\n";
  }

  // Cleanup
  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    ACK::ErrorCode ack=vehicle->subscribe->removePackage(pkgIndex, timeout);

    if (ACK::getError(ack)) {
      std::cout << "Error unsubscribing; please restart the drone/FC to get back "
        "to a clean state.\n";
    }
  }

  return true;
}

/*! Position Control. Allows you to set an offset from your current
    location. The aircraft will move to that position and stay there.
    Typical use would be as a building block in an outer loop that does not
    require many fast changes, perhaps a few-waypoint trajectory. For smoother
    transition and response you should convert your trajectory to attitude
    setpoints and use attitude control or convert to velocity setpoints
    and use velocity control.
!*/
bool
moveByPositionOffset(Vehicle *vehicle, float xOffsetDesired,
                     float yOffsetDesired, float zOffsetDesired,
                     float yawDesired, float posThresholdInM,
                     float yawThresholdInDeg)
{
  // Set timeout: this timeout is the time you allow the drone to take to finish
  // the
  // mission
  int responseTimeout              = 1;
  int timeoutInMilSec              = 20000;
  int controlFreqInHz              = 50; // Hz
  int cycleTimeInMs                = 1000 / controlFreqInHz;
  int outOfControlBoundsTimeLimit  = 10 * cycleTimeInMs; // 10 cycles
  int withinControlBoundsTimeReqmt = 50 * cycleTimeInMs; // 50 cycles
  int pkgIndex;

  //@todo: remove this once the getErrorCode function signature changes
  char func[50];

  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    // Telemetry: Verify the subscription
    ACK::ErrorCode subscribeStatus;
    subscribeStatus = vehicle->subscribe->verify(responseTimeout);
    if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
    {
      ACK::getErrorCodeMessage(subscribeStatus, func);
      return false;
    }

    // Telemetry: Subscribe to quaternion, fused lat/lon and altitude at freq 50
    // Hz
    pkgIndex                  = 0;
    int       freq            = 50;
    TopicName topicList50Hz[] = { TOPIC_QUATERNION, TOPIC_GPS_FUSED };
    int       numTopic = sizeof(topicList50Hz) / sizeof(topicList50Hz[0]);
    bool      enableTimestamp = false;

    bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(
      pkgIndex, numTopic, topicList50Hz, enableTimestamp, freq);
    if (!(pkgStatus))
    {
      return pkgStatus;
    }
    subscribeStatus =
      vehicle->subscribe->startPackage(pkgIndex, responseTimeout);
    if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
    {
      ACK::getErrorCodeMessage(subscribeStatus, func);
      // Cleanup before return
      vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
      return false;
    }

    // Also, since we don't have a source for relative height through subscription,
    // start using broadcast height
    if (!startGlobalPositionBroadcast(vehicle))
    {
      // Cleanup before return
      vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
      return false;
    }
  }

  // Wait for data to come in
  sleep(1);

  // Get data

  // Global position retrieved via subscription
  Telemetry::TypeMap<TOPIC_GPS_FUSED>::type currentSubscriptionGPS;
  Telemetry::TypeMap<TOPIC_GPS_FUSED>::type originSubscriptionGPS;
  // Global position retrieved via broadcast
  Telemetry::GlobalPosition currentBroadcastGP;
  Telemetry::GlobalPosition originBroadcastGP;

  // Convert position offset from first position to local coordinates
  Telemetry::Vector3f localOffset;

  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    currentSubscriptionGPS = vehicle->subscribe->getValue<TOPIC_GPS_FUSED>();
    originSubscriptionGPS  = currentSubscriptionGPS;
    localOffsetFromGpsOffset(vehicle, localOffset,
                             static_cast<void*>(&currentSubscriptionGPS),
                             static_cast<void*>(&originSubscriptionGPS));

    // Get the broadcast GP since we need the height for zCmd
    currentBroadcastGP = vehicle->broadcast->getGlobalPosition();
  }
  else
  {
    currentBroadcastGP = vehicle->broadcast->getGlobalPosition();
    originBroadcastGP  = currentBroadcastGP;
    localOffsetFromGpsOffset(vehicle, localOffset,
                             static_cast<void*>(&currentBroadcastGP),
                             static_cast<void*>(&originBroadcastGP));
  }

  // Get initial offset. We will update this in a loop later.
  double xOffsetRemaining = xOffsetDesired - localOffset.x;
  double yOffsetRemaining = yOffsetDesired - localOffset.y;
  double zOffsetRemaining = zOffsetDesired - localOffset.z;

  // Conversions
  double yawDesiredRad     = DEG2RAD * yawDesired;
  double yawThresholdInRad = DEG2RAD * yawThresholdInDeg;

  //! Get Euler angle

  // Quaternion retrieved via subscription
  Telemetry::TypeMap<TOPIC_QUATERNION>::type subscriptionQ;
  // Quaternion retrieved via broadcast
  Telemetry::Quaternion broadcastQ;

  double yawInRad;
  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    subscriptionQ = vehicle->subscribe->getValue<TOPIC_QUATERNION>();
    yawInRad = toEulerAngle((static_cast<void*>(&subscriptionQ))).z / DEG2RAD;
  }
  else
  {
    broadcastQ = vehicle->broadcast->getQuaternion();
    yawInRad   = toEulerAngle((static_cast<void*>(&broadcastQ))).z / DEG2RAD;
  }

  int   elapsedTimeInMs     = 0;
  int   withinBoundsCounter = 0;
  int   outOfBounds         = 0;
  int   brakeCounter        = 0;
  int   speedFactor         = 2;
  float xCmd, yCmd, zCmd;
  // There is a deadband in position control
  // the z cmd is absolute height
  // while x and y are in relative
  float zDeadband = 0.12;

  if (vehicle->isM100() || vehicle->isLegacyM600())
  {
    zDeadband = 0.12 * 10;
  }

  /*! Calculate the inputs to send the position controller. We implement basic
   *  receding setpoint position control and the setpoint is always 1 m away
   *  from the current position - until we get within a threshold of the goal.
   *  From that point on, we send the remaining distance as the setpoint.
   */
  if (xOffsetDesired > 0)
    xCmd = (xOffsetDesired < speedFactor) ? xOffsetDesired : speedFactor;
  else if (xOffsetDesired < 0)
    xCmd =
      (xOffsetDesired > -1 * speedFactor) ? xOffsetDesired : -1 * speedFactor;
  else
    xCmd = 0;

  if (yOffsetDesired > 0)
    yCmd = (yOffsetDesired < speedFactor) ? yOffsetDesired : speedFactor;
  else if (yOffsetDesired < 0)
    yCmd =
      (yOffsetDesired > -1 * speedFactor) ? yOffsetDesired : -1 * speedFactor;
  else
    yCmd = 0;

  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    zCmd = currentBroadcastGP.height + zOffsetDesired; //Since subscription cannot give us a relative height, use broadcast.
  }
  else
  {
    zCmd = currentBroadcastGP.height + zOffsetDesired;
  }

  //! Main closed-loop receding setpoint position control
  while (elapsedTimeInMs < timeoutInMilSec)
  {
    vehicle->control->positionAndYawCtrl(xCmd, yCmd, zCmd,
                                         yawDesiredRad / DEG2RAD);

    usleep(cycleTimeInMs * 1000);
    elapsedTimeInMs += cycleTimeInMs;

    //! Get current position in required coordinates and units
    if (!vehicle->isM100() && !vehicle->isLegacyM600())
    {
      subscriptionQ = vehicle->subscribe->getValue<TOPIC_QUATERNION>();
      yawInRad      = toEulerAngle((static_cast<void*>(&subscriptionQ))).z;
      currentSubscriptionGPS = vehicle->subscribe->getValue<TOPIC_GPS_FUSED>();
      localOffsetFromGpsOffset(vehicle, localOffset,
                               static_cast<void*>(&currentSubscriptionGPS),
                               static_cast<void*>(&originSubscriptionGPS));

      // Get the broadcast GP since we need the height for zCmd
      currentBroadcastGP = vehicle->broadcast->getGlobalPosition();
    }
    else
    {
      broadcastQ         = vehicle->broadcast->getQuaternion();
      yawInRad           = toEulerAngle((static_cast<void*>(&broadcastQ))).z;
      currentBroadcastGP = vehicle->broadcast->getGlobalPosition();
      localOffsetFromGpsOffset(vehicle, localOffset,
                               static_cast<void*>(&currentBroadcastGP),
                               static_cast<void*>(&originBroadcastGP));
    }

    //! See how much farther we have to go
    xOffsetRemaining = xOffsetDesired - localOffset.x;
    yOffsetRemaining = yOffsetDesired - localOffset.y;
    zOffsetRemaining = zOffsetDesired - localOffset.z;

    //! See if we need to modify the setpoint
    if (std::abs(xOffsetRemaining) < speedFactor)
    {
      xCmd = xOffsetRemaining;
    }
    if (std::abs(yOffsetRemaining) < speedFactor)
    {
      yCmd = yOffsetRemaining;
    }

    if (vehicle->isM100() && std::abs(xOffsetRemaining) < posThresholdInM &&
        std::abs(yOffsetRemaining) < posThresholdInM &&
        std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad)
    {
      //! 1. We are within bounds; start incrementing our in-bound counter
      withinBoundsCounter += cycleTimeInMs;
    }
    else if (std::abs(xOffsetRemaining) < posThresholdInM &&
             std::abs(yOffsetRemaining) < posThresholdInM &&
             std::abs(zOffsetRemaining) < zDeadband &&
             std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad)
    {
      //! 1. We are within bounds; start incrementing our in-bound counter
      withinBoundsCounter += cycleTimeInMs;
    }
    else
    {
      if (withinBoundsCounter != 0)
      {
        //! 2. Start incrementing an out-of-bounds counter
        outOfBounds += cycleTimeInMs;
      }
    }
    //! 3. Reset withinBoundsCounter if necessary
    if (outOfBounds > outOfControlBoundsTimeLimit)
    {
      withinBoundsCounter = 0;
      outOfBounds         = 0;
    }
    //! 4. If within bounds, set flag and break
    if (withinBoundsCounter >= withinControlBoundsTimeReqmt)
    {
      break;
    }
  }

  //! Set velocity to zero, to prevent any residual velocity from position
  //! command
  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    while (brakeCounter < withinControlBoundsTimeReqmt)
    {
      vehicle->control->emergencyBrake();
      usleep(cycleTimeInMs * 10);
      brakeCounter += cycleTimeInMs;
    }
  }

  if (elapsedTimeInMs >= timeoutInMilSec)
  {
    std::cout << "Task timeout!\n";
    if (!vehicle->isM100() && !vehicle->isLegacyM600())
    {
      ACK::ErrorCode ack =
        vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
      if (ACK::getError(ack))
      {
        std::cout << "Error unsubscribing; please restart the drone/FC to get "
                     "back to a clean state.\n";
      }
    }
    return ACK::FAIL;
  }

  if (!vehicle->isM100() && !vehicle->isLegacyM600())
  {
    ACK::ErrorCode ack =
      vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    if (ACK::getError(ack))
    {
      std::cout
        << "Error unsubscribing; please restart the drone/FC to get back "
           "to a clean state.\n";
    }
  }

  return ACK::SUCCESS;
}

/*! Monitored Takeoff (Blocking API call). Return status as well as ack.
    This version of takeoff makes sure your aircraft actually took off
    and only returns when takeoff is complete.
    Use unless you want to do other stuff during takeoff - this will block
    the main thread.
!*/
bool monitoredLanding(Vehicle* vehicle, int timeout) {
  //@todo: remove this once the getErrorCode function signature changes
  char func[50];
  int pkgIndex;

  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    // Telemetry: Verify the subscription
    ACK::ErrorCode subscribeStatus;
    subscribeStatus=vehicle->subscribe->verify(timeout);

    if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
      ACK::getErrorCodeMessage(subscribeStatus, func);
      return false;
    }

    // Telemetry: Subscribe to flight status and mode at freq 10 Hz
    pkgIndex=0;
    int freq=10;

    TopicName topicList10Hz[]= {
      TOPIC_STATUS_FLIGHT,
      TOPIC_STATUS_DISPLAYMODE
    }

    ;
    int numTopic=sizeof(topicList10Hz) / sizeof(topicList10Hz[0]);
    bool enableTimestamp=false;

    bool pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList10Hz, enableTimestamp, freq);

    if ( !(pkgStatus)) {
      return pkgStatus;
    }

    subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, timeout);

    if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
      ACK::getErrorCodeMessage(subscribeStatus, func);
      // Cleanup before return
      vehicle->subscribe->removePackage(pkgIndex, timeout);
      return false;
    }
  }

  // Start landing
  ACK::ErrorCode landingStatus=vehicle->control->land(timeout);

  if (ACK::getError(landingStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(landingStatus, func);
    return false;
  }

  // First check: Landing started
  int landingNotStarted=0;
  int timeoutCycles=20;

  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    while (vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_AUTO_LANDING && landingNotStarted < timeoutCycles) {
      landingNotStarted++;
      usleep(100000);
    }
  }

  // else if (vehicle->isM100()) {
  //   while (vehicle->broadcast->getStatus().flight !=DJI::OSDK::VehicleStatus::M100FlightStatus::LANDING && landingNotStarted < timeoutCycles) {
  //     landingNotStarted++;
  //     usleep(100000);
  //   }
  // }

  if (landingNotStarted==timeoutCycles) {
    std::cout << "Landing failed. Aircraft is still in the air."<< std::endl;

    if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
      // Cleanup before return
      ACK::ErrorCode ack=vehicle->subscribe->removePackage(pkgIndex, timeout);

      if (ACK::getError(ack)) {
        std::cout << "Error unsubscribing; please restart the drone/FC to get "
          "back to a clean state.\n";
      }
    }

    return false;
  }

  else {
    std::cout << "Landing...\n";
  }

  // Second check: Finished landing
  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    while (vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>()==VehicleStatus::DisplayMode::MODE_AUTO_LANDING && vehicle->subscribe->getValue<TOPIC_STATUS_FLIGHT>()==VehicleStatus::FlightStatus::IN_AIR) {
      sleep(1);
    }

    if (vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_P_GPS || vehicle->subscribe->getValue<TOPIC_STATUS_DISPLAYMODE>() !=VehicleStatus::DisplayMode::MODE_ATTITUDE) {
      std::cout << "Successful landing!\n";
    }

    else {
      std::cout << "Landing finished, but the aircraft is in an unexpected mode. "
        "Please connect DJI GO.\n";
      ACK::ErrorCode ack=vehicle->subscribe->removePackage(pkgIndex, timeout);

      if (ACK::getError(ack)) {
        std::cout << "Error unsubscribing; please restart the drone/FC to get "
          "back to a clean state.\n";
      }

      return false;
    }
  }

  else if (vehicle->isLegacyM600()) {
    while (vehicle->broadcast->getStatus().flight > DJI::OSDK::VehicleStatus::FlightStatus::STOPED) {
      sleep(1);
    }

    Telemetry::GlobalPosition gp;

    do {
      sleep(2);
      gp=vehicle->broadcast->getGlobalPosition();
    }

    while (gp.altitude !=0);

    if (gp.altitude !=0) {
      std::cout << "Landing finished, but the aircraft is in an unexpected mode. "
        "Please connect DJI GO.\n";
      return false;
    }

    else {
      std::cout << "Successful landing!\n";
    }
  }

  else // M100

    {
    while (vehicle->broadcast->getStatus().flight==DJI::OSDK::VehicleStatus::M100FlightStatus::FINISHING_LANDING) {
      sleep(1);
    }

    Telemetry::GlobalPosition gp;

    do {
      sleep(2);
      gp=vehicle->broadcast->getGlobalPosition();
    }

    while (gp.altitude !=0);

    if (gp.altitude !=0) {
      std::cout << "Landing finished, but the aircraft is in an unexpected mode. "
        "Please connect DJI GO.\n";
      return false;
    }

    else {
      std::cout << "Successful landing!\n";
    }
  }

  // Cleanup
  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    ACK::ErrorCode ack=vehicle->subscribe->removePackage(pkgIndex, timeout);

    if (ACK::getError(ack)) {
      std::cout << "Error unsubscribing; please restart the drone/FC to get back "
        "to a clean state.\n";
    }
  }

  return true;
}

// Helper Functions

/*! Very simple calculation of local NED offset between two pairs of GPS
/coordinates.
    Accurate when distances are small.
!*/
void localOffsetFromGpsOffset(Vehicle* vehicle, Telemetry::Vector3f& deltaNed,
  void* target, void* origin) {
  Telemetry::GPSFused* subscriptionTarget;
  Telemetry::GPSFused* subscriptionOrigin;
  Telemetry::GlobalPosition* broadcastTarget;
  Telemetry::GlobalPosition* broadcastOrigin;
  double deltaLon;
  double deltaLat;

  if ( !vehicle->isM100() && !vehicle->isLegacyM600()) {
    subscriptionTarget=(Telemetry::GPSFused*)target;
    subscriptionOrigin=(Telemetry::GPSFused*)origin;
    deltaLon=subscriptionTarget->longitude - subscriptionOrigin->longitude;
    deltaLat=subscriptionTarget->latitude - subscriptionOrigin->latitude;
    deltaNed.x=deltaLat * C_EARTH;
    deltaNed.y=deltaLon * C_EARTH * cos(subscriptionTarget->latitude);
    deltaNed.z=subscriptionTarget->altitude - subscriptionOrigin->altitude;
  }

  else {
    broadcastTarget=(Telemetry::GlobalPosition*)target;
    broadcastOrigin=(Telemetry::GlobalPosition*)origin;
    deltaLon=broadcastTarget->longitude - broadcastOrigin->longitude;
    deltaLat=broadcastTarget->latitude - broadcastOrigin->latitude;
    deltaNed.x=deltaLat * C_EARTH;
    deltaNed.y=deltaLon * C_EARTH * cos(broadcastTarget->latitude);
    deltaNed.z=broadcastTarget->altitude - broadcastOrigin->altitude;
  }
}

Telemetry::Vector3f toEulerAngle(void* quaternionData) {Telemetry::Vector3f ans;  Telemetry::Quaternion* quaternion=(Telemetry::Quaternion*)quaternionData;

  double q2sqr=quaternion->q2 * quaternion->q2;
  double t0=-2.0 * (q2sqr + quaternion->q3 * quaternion->q3)+1.0;
  double t1=+2.0 * (quaternion->q1 * quaternion->q2 + quaternion->q0 * quaternion->q3);
  double t2=-2.0 * (quaternion->q1 * quaternion->q3 - quaternion->q0 * quaternion->q2);
  double t3=+2.0 * (quaternion->q2 * quaternion->q3 + quaternion->q0 * quaternion->q1);
  double t4=-2.0 * (quaternion->q1 * quaternion->q1 + q2sqr)+1.0;

  t2=(t2 > 1.0) ? 1.0: t2;
  t2=(t2 < -1.0) ? -1.0: t2;

  ans.x=asin(t2);
  ans.y=atan2(t3, t4);
  ans.z=atan2(t1, t0);

  return ans;
}

bool startGlobalPositionBroadcast(Vehicle* vehicle) {
  uint8_t freq[16];

  /* Channels definition for A3/N3/M600
   * 0 - Timestamp
   * 1 - Attitude Quaternions
   * 2 - Acceleration
   * 3 - Velocity (Ground Frame)
   * 4 - Angular Velocity (Body Frame)
   * 5 - Position
   * 6 - GPS Detailed Information
   * 7 - RTK Detailed Information
   * 8 - Magnetometer
   * 9 - RC Channels Data
   * 10 - Gimbal Data
   * 11 - Flight Status
   * 12 - Battery Level
   * 13 - Control Information
   */
  freq[0]=DataBroadcast::FREQ_HOLD;
  freq[1]=DataBroadcast::FREQ_HOLD;
  freq[2]=DataBroadcast::FREQ_HOLD;
  freq[3]=DataBroadcast::FREQ_HOLD;
  freq[4]=DataBroadcast::FREQ_HOLD;
  freq[5]=DataBroadcast::FREQ_50HZ; // This is the only one we want to change
  freq[6]=DataBroadcast::FREQ_HOLD;
  freq[7]=DataBroadcast::FREQ_HOLD;
  freq[8]=DataBroadcast::FREQ_HOLD;
  freq[9]=DataBroadcast::FREQ_HOLD;
  freq[10]=DataBroadcast::FREQ_HOLD;
  freq[11]=DataBroadcast::FREQ_HOLD;
  freq[12]=DataBroadcast::FREQ_HOLD;
  freq[13]=DataBroadcast::FREQ_HOLD;

  ACK::ErrorCode ack=vehicle->broadcast->setBroadcastFreq(freq, 1);

  if (ACK::getError(ack)) {
    ACK::getErrorCodeMessage(ack, __func__);
    return false;
  }

  else {
    return true;
  }
}

/**
 * @brief 
 * 
 * @param vehicle 
 * @param responseTimeout 
 * @return the value 0stand for s mode,7772 stand for p mode,-7833 stand for a mode 
 *          
 */
int getRCmode(DJI::OSDK::Vehicle* vehicle,int *rcMode, int responseTimeout){
  ACK::ErrorCode subscribeStatus;
  subscribeStatus=vehicle->subscribe->verify(responseTimeout);
  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    return false;
  }

  int pkgIndex                  = 2;
  int freq                      = 50;
  TopicName topicList50Hz[] = { TOPIC_RC, TOPIC_VELOCITY };
  int numTopic                  = sizeof(topicList50Hz) / sizeof(topicList50Hz[0]);
  bool enableTimestamp           = false;

  bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(
    pkgIndex, numTopic, topicList50Hz, enableTimestamp, freq);
  if (!(pkgStatus))
  {
    return pkgStatus;
  }
  subscribeStatus = vehicle->subscribe->startPackage(pkgIndex, responseTimeout);
  if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
  {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    // Cleanup before return
    vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    return false;
  }
  // sleep(1);//用于等待大量数据进入
  TypeMap<TOPIC_RC>::type rc;
  rc = vehicle->subscribe->getValue<TOPIC_RC>();
  usleep(5000);
  *rcMode = rc.mode;
  vehicle->subscribe->removePackage(2, responseTimeout);
}

/**
 * @brief 
 * 
 * @param vehicle 
 * @param responseTimeout 
 * @return true 
 * @return false 
 */
int getSwitchCallFromSbus(DJI::OSDK::Vehicle* vehicle, int16_t *rcMode, int responseTimeout){
  ACK::ErrorCode subscribeStatus;
  subscribeStatus=vehicle->subscribe->verify(responseTimeout);
  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    return false;
  }

  int pkgIndex                  = 2;
  int freq                      = 50;
  TopicName topicList50Hz[] = {TOPIC_RC_FULL_RAW_DATA};
  int numTopic                  = sizeof(topicList50Hz) / sizeof(topicList50Hz[0]);
  bool enableTimestamp           = false;

  bool pkgStatus = vehicle->subscribe->initPackageFromTopicList(
    pkgIndex, numTopic, topicList50Hz, enableTimestamp, freq);
  if (!(pkgStatus))
  {
    return pkgStatus;
  }
  subscribeStatus = vehicle->subscribe->startPackage(pkgIndex, responseTimeout);
  if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
  {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    // Cleanup before return
    vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    return false;
  }
  sleep(1);
  TypeMap<TOPIC_RC_FULL_RAW_DATA>::type RCFullRawData;
  RCFullRawData = vehicle->subscribe->getValue<TOPIC_RC_FULL_RAW_DATA>();
  usleep(5000);
  rcMode = RCFullRawData.sbus.data;//这里的Sbus信号时已经处理过的16个通道，每个通道都是int16
  vehicle->subscribe->removePackage(2, responseTimeout);
  return 0;
}
/**
 * @brief 
 * 
 * @param vehicle 
 * @param xOffsetDesired 
 * @param yOffsetDesired 
 * @param zOffsetDesired 
 * @param yawDesired 
 * @param posThresholdInM 
 * @param yawThresholdInDeg 
 * @return int 
 */
// int moveByVelocityAndAngle(DJI::OSDK::Vehicle *vehicle, float xOffsetDesired,
//     float yOffsetDesired, float zOffsetDesired,
//     float yawDesired, float posThresholdInM,
//     float yawThresholdInDeg){
//       // Set timeout: this timeout is the time you allow the drone to take to finish
//   // the mission
//   int responseTimeout              = 1;
//   int timeoutInMilSec              = 20000;
//   int controlFreqInHz              = 50; // Hz控制频率
//   int cycleTimeInMs                = 1000 / controlFreqInHz//利用单次loop代替控制频率的作用;
//   int outOfControlBoundsTimeLimit  = 10 * cycleTimeInMs; // 10 loop出圈之后重新飞行
//   int withinControlBoundsTimeReqmt = 50 * cycleTimeInMs; // 50 loop在圈中停止控制
//   int pkgIndex;

//   //@todo: remove this once the getErrorCode function signature changes
//   char func[50];
//     // Telemetry: Verify the subscription
//     ACK::ErrorCode subscribeStatus;
//     subscribeStatus = vehicle->subscribe->verify(responseTimeout);
//     if (ACK::getError(subscribeStatus) != ACK::SUCCESS)
//     {
//       ACK::getErrorCodeMessage(subscribeStatus, func);
//       return false;
//     }

//   sleep(1);

//   // 找到还需要控制的目标量，位置，在下面需要更新
//   double xOffsetRemaining = xOffsetDesired - localOffset.x;
//   double yOffsetRemaining = yOffsetDesired - localOffset.y;
//   double zOffsetRemaining = zOffsetDesired - localOffset.z;

//   // Yaw数据时DEG形式的，为了控制需要改称RAD形式，但是往下没有用到，奇，Conversions
//   double yawDesiredRad     = DEG2RAD * yawDesired;
//   double yawThresholdInRad = DEG2RAD * yawThresholdInDeg;

//   int   elapsedTimeInMs     = 0;//已经过去的时间
//   int   withinBoundsCounter = 0;//在阈值内的时间
//   int   outOfBounds         = 0;//超出阈值的时间
//   int   brakeCounter        = 0;
//   int   speedFactor         = 2;//小于该值才会更新控制指令
//   float xCmd, yCmd, zCmd;
//   // There is a deadband in position control
//   // the z cmd is absolute height
//   // while x and y are in relative
//   float zDeadband = 0.12;
//   /*! Calculate the inputs to send the position controller. We implement basic
//    *  receding setpoint position control and the setpoint is always 1 m away
//    *  from the current position - until we get within a threshold of the goal.
//    *  From that point on, we send the remaining distance as the setpoint.
//    */


//   //第一次控制
//   if (xOffsetDesired > 0)
//     xCmd = (xOffsetDesired < speedFactor) ? xOffsetDesired : speedFactor;
//   else if (xOffsetDesired < 0)
//     xCmd =
//       (xOffsetDesired > -1 * speedFactor) ? xOffsetDesired : -1 * speedFactor;
//   else
//     xCmd = 0;

//   if (yOffsetDesired > 0)
//     yCmd = (yOffsetDesired < speedFactor) ? yOffsetDesired : speedFactor;
//   else if (yOffsetDesired < 0)
//     yCmd =
//       (yOffsetDesired > -1 * speedFactor) ? yOffsetDesired : -1 * speedFactor;
//   else
//     yCmd = 0;

//   if (!vehicle->isM100() && !vehicle->isLegacyM600())
//   {
//     zCmd = currentBroadcastGP.height + zOffsetDesired; //Since subscription cannot give us a relative height, use broadcast.
//   }
//   else
//   {
//     zCmd = currentBroadcastGP.height + zOffsetDesired;
//   }

//   //更新数据闭环控制
//   //! Main closed-loop receding setpoint position control
//   while (elapsedTimeInMs < timeoutInMilSec)
//   {
//     vehicle->control->positionAndYawCtrl(xCmd, yCmd, zCmd,yawDesiredRad / DEG2RAD);

//     usleep(cycleTimeInMs * 1000);//你为什么药用usleep不用sleep，哦太小了
//     elapsedTimeInMs += cycleTimeInMs;

//     //! See how much farther we have to go
//     xOffsetRemaining = xOffsetDesired - localOffset.x;
//     yOffsetRemaining = yOffsetDesired - localOffset.y;
//     zOffsetRemaining = zOffsetDesired - localOffset.z;

//     //! See if we need to modify the setpoint
//     if (std::abs(xOffsetRemaining) < speedFactor)
//     {
//       xCmd = xOffsetRemaining;
//     }
//     if (std::abs(yOffsetRemaining) < speedFactor)
//     {
//       yCmd = yOffsetRemaining;
//     }

//     if (vehicle->isM100() && std::abs(xOffsetRemaining) < posThresholdInM &&
//         std::abs(yOffsetRemaining) < posThresholdInM &&
//         std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad)
//     {
//       //! 1. We are within bounds; start incrementing our in-bound counter
//       withinBoundsCounter += cycleTimeInMs;
//     }
//     else if (std::abs(xOffsetRemaining) < posThresholdInM &&
//              std::abs(yOffsetRemaining) < posThresholdInM &&
//              std::abs(zOffsetRemaining) < zDeadband &&
//              std::abs(yawInRad - yawDesiredRad) < yawThresholdInRad)
//     {
//       //! 1. We are within bounds; start incrementing our in-bound counter
//       withinBoundsCounter += cycleTimeInMs;
//     }
//     else
//     {
//       if (withinBoundsCounter != 0)
//       {
//         //! 2. Start incrementing an out-of-bounds counter
//         outOfBounds += cycleTimeInMs;
//       }
//     }
//     //! 3. Reset withinBoundsCounter if necessary
//     if (outOfBounds > outOfControlBoundsTimeLimit)
//     {
//       withinBoundsCounter = 0;
//       outOfBounds         = 0;
//     }
//     //! 4. If within bounds, set flag and break
//     if (withinBoundsCounter >= withinControlBoundsTimeReqmt)
//     {
//       break;
//     }
//   }

//   //! 任务完成，设置速度为零
//   if (!vehicle->isM100() && !vehicle->isLegacyM600())
//   {
//     while (brakeCounter < withinControlBoundsTimeReqmt)
//     {
//       vehicle->control->emergencyBrake();
//       usleep(cycleTimeInMs * 10);
//       brakeCounter += cycleTimeInMs;
//     }
//   }
//   //超时，未完成任务
//   if (elapsedTimeInMs >= timeoutInMilSec)
//   {
//     std::cout << "Task timeout!\n";
//     if (!vehicle->isM100() && !vehicle->isLegacyM600())
//     {
//       ACK::ErrorCode ack =
//         vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
//       if (ACK::getError(ack))
//       {
//         std::cout << "Error unsubscribing; please restart the drone/FC to get "
//                      "back to a clean state.\n";
//       }
//     }
//     return ACK::FAIL;
//   }
//   //清除包
//   if (!vehicle->isM100() && !vehicle->isLegacyM600())
//   {
//     ACK::ErrorCode ack = vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
//     if (ACK::getError(ack))
//     {
//       std::cout<< "Error unsubscribing; please restart the drone/FC to get back "<<"to a clean state.\n";
//     }
//   }
//   return ACK::SUCCESS;
// }


bool subscribeToData(Vehicle* vehicle, int responseTimeout) {

  // RTK can be detected as unavailable only for Flight controllers that don't support RTK
  bool rtkAvailable=false;
  // Counters
  int elapsedTimeInMs=0;
  int timeToPrintInMs=20000;

  // We will subscribe to six kinds of data:
  // 1. Flight Status at 1 Hz
  // 2. Fused Lat/Lon at 10Hz
  // 3. Fused Altitude at 10Hz
  // 4. RC Channels at 50 Hz
  // 5. Velocity at 50 Hz
  // 6. Quaternion at 200 Hz

  // Please make sure your drone is in simulation mode. You can fly the drone
  // with your RC to
  // get different values.

  // Telemetry: Verify the subscription
  ACK::ErrorCode subscribeStatus;
  subscribeStatus=vehicle->subscribe->verify(responseTimeout);

  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    return false;
  }




  // Package 0: Subscribe to flight status at freq 1 Hz
  int pkgIndex=0;
  int freq=1;

  TopicName topicList1Hz[]= {
    TOPIC_STATUS_FLIGHT
  }

  ;
  int numTopic=sizeof(topicList1Hz) / sizeof(topicList1Hz[0]);
  bool enableTimestamp=false;

  bool pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList1Hz, enableTimestamp, freq);

  if ( !(pkgStatus)) {
    return pkgStatus;
  }

  subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, responseTimeout);

  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    // Cleanup before return
    vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    return false;
  }




  // Package 1: Subscribe to Lat/Lon, and Alt at freq 10 Hz
  pkgIndex=1;
  freq=10;

  TopicName topicList10Hz[]= {
    TOPIC_GPS_FUSED,
    TOPIC_ALTITUDE_FUSIONED
  }

  ;
  numTopic=sizeof(topicList10Hz) / sizeof(topicList10Hz[0]);
  enableTimestamp=false;

  pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList10Hz, enableTimestamp, freq);

  if ( !(pkgStatus)) {
    return pkgStatus;
  }

  subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, responseTimeout);

  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    // Cleanup before return
    vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    return false;
  }




  // Package 2: Subscribe to RC Channel and Velocity at freq 50 Hz
  pkgIndex=2;
  freq=50;

  TopicName topicList50Hz[]= {
    TOPIC_RC,
    TOPIC_VELOCITY
  }

  ;
  numTopic=sizeof(topicList50Hz) / sizeof(topicList50Hz[0]);
  enableTimestamp=false;

  pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList50Hz, enableTimestamp, freq);

  if ( !(pkgStatus)) {
    return pkgStatus;
  }

  subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, responseTimeout);

  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    // Cleanup before return
    vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    return false;
  }





  // Package 3: Subscribe to Quaternion at freq 200 Hz.
  pkgIndex=3;
  freq=200;

  TopicName topicList200Hz[]= {
    TOPIC_QUATERNION
  }

  ;
  numTopic=sizeof(topicList200Hz) / sizeof(topicList200Hz[0]);
  enableTimestamp=false;

  pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicList200Hz, enableTimestamp, freq);

  if ( !(pkgStatus)) {
    return pkgStatus;
  }

  subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, responseTimeout);

  if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
    ACK::getErrorCodeMessage(subscribeStatus, __func__);
    // Cleanup before return
    vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
    return false;
  }





  // Package 4: Subscribe to RTK at freq 5 Hz.
  pkgIndex=4;
  freq=5;

  TopicName topicListRTK5Hz[]= {
    TOPIC_RTK_POSITION,
    TOPIC_RTK_YAW_INFO,
    TOPIC_RTK_POSITION_INFO,
    TOPIC_RTK_VELOCITY,
    TOPIC_RTK_YAW
  }

  ;
  numTopic=sizeof(topicListRTK5Hz) / sizeof(topicListRTK5Hz[0]);
  enableTimestamp=false;

  pkgStatus=vehicle->subscribe->initPackageFromTopicList(pkgIndex, numTopic, topicListRTK5Hz, enableTimestamp, freq);

  if ( !(pkgStatus)) {
    return pkgStatus;
  }

  else {
    subscribeStatus=vehicle->subscribe->startPackage(pkgIndex, responseTimeout);

    if(subscribeStatus.data==ErrorCode::SubscribeACK::SOURCE_DEVICE_OFFLINE) {
      std::cout << "RTK Not Available"<< "\n";
      rtkAvailable=false;
    }

    else {
      rtkAvailable=true;

      if (ACK::getError(subscribeStatus) !=ACK::SUCCESS) {
        ACK::getErrorCodeMessage(subscribeStatus, __func__);
        // Cleanup before return
        vehicle->subscribe->removePackage(pkgIndex, responseTimeout);
        return false;
      }
    }
  }




  // Wait for the data to start coming in.
  sleep(1);

  // Get all the data once before the loop to initialize vars
  TypeMap<TOPIC_STATUS_FLIGHT>::type flightStatus;
  TypeMap<TOPIC_GPS_FUSED>::type latLon;
  TypeMap<TOPIC_ALTITUDE_FUSIONED>::type altitude;
  TypeMap<TOPIC_RC>::type rc;
  TypeMap<TOPIC_VELOCITY>::type velocity;
  TypeMap<TOPIC_QUATERNION>::type quaternion;
  TypeMap<TOPIC_RTK_POSITION>::type rtk;
  TypeMap<TOPIC_RTK_POSITION_INFO>::type rtk_pos_info;
  TypeMap<TOPIC_RTK_VELOCITY>::type rtk_velocity;
  TypeMap<TOPIC_RTK_YAW>::type rtk_yaw;
  TypeMap<TOPIC_RTK_YAW_INFO>::type rtk_yaw_info;

  // Print in a loop for 2 sec
  while (elapsedTimeInMs < timeToPrintInMs) {
    flightStatus=vehicle->subscribe->getValue<TOPIC_STATUS_FLIGHT>();
    latLon=vehicle->subscribe->getValue<TOPIC_GPS_FUSED>();
    altitude=vehicle->subscribe->getValue<TOPIC_ALTITUDE_FUSIONED>();
    rc=vehicle->subscribe->getValue<TOPIC_RC>();
    velocity=vehicle->subscribe->getValue<TOPIC_VELOCITY>();
    quaternion=vehicle->subscribe->getValue<TOPIC_QUATERNION>();

    if(rtkAvailable) {
      rtk=vehicle->subscribe->getValue<TOPIC_RTK_POSITION>();
      rtk_pos_info=vehicle->subscribe->getValue<TOPIC_RTK_POSITION_INFO>();
      rtk_velocity=vehicle->subscribe->getValue<TOPIC_RTK_VELOCITY>();
      rtk_yaw=vehicle->subscribe->getValue<TOPIC_RTK_YAW>();
      rtk_yaw_info=vehicle->subscribe->getValue<TOPIC_RTK_YAW_INFO>();
    }  


    if(rtkAvailable) {}
    std::cout << "-------\n\n";
    usleep(5000);
    elapsedTimeInMs+=5;
  }

  std::cout << "Done printing!\n";
  vehicle->subscribe->removePackage(0, responseTimeout);
  vehicle->subscribe->removePackage(1, responseTimeout);
  vehicle->subscribe->removePackage(2, responseTimeout);
  vehicle->subscribe->removePackage(3, responseTimeout);
  vehicle->subscribe->removePackage(4, responseTimeout);

  return true;
}


